/**
 * @file SamplingPlanner.h
 * @brief 
 * @author Linfu Wei (ghowoght@qq.com)
 * @version 1.0
 * @date 2021-02-18
 * 
 * @copyright Copyright (c) 2021  WHU-EIS
 * 
 */
#include <ros/ros.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <costmap_2d/costmap_2d.h>
#include <nav_core/base_global_planner.h>
#include <geometry_msgs/PoseStamped.h>
#include <angles/angles.h>
#include <base_local_planner/world_model.h>
#include <base_local_planner/costmap_model.h>

#include <eigen3/Eigen/StdVector>
#include <globalplanner/BLH2locpos.hpp>

using al_topo_map::BLH2locpos;

#ifndef _SAMPLING_PLANNER_H
#define _SAMPLING_PLANNER_H

namespace global_planner{
    class SamplingPlanner : public nav_core::BaseGlobalPlanner{
    public:
        SamplingPlanner();
        SamplingPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros);

        // 重载BaseGlobalPlanner里的接口
        void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
        bool makePlan(  const geometry_msgs::PoseStamped& start,
                        const geometry_msgs::PoseStamped& goal,
                        std::vector<geometry_msgs::PoseStamped>& plan
                        );
        void publishPlan(const std::vector<geometry_msgs::PoseStamped>& path);
        void publishMultiPlan(const std::vector<std::vector<geometry_msgs::PoseStamped>>& paths);
        bool worldToMap(double wx, double wy, double& mx, double& my);
        void mapToWorld(double mx, double my, double& wx, double& wy); 
    private:
        bool initialized_;
        ros::Publisher plan_pub_;
        ros::Publisher multiplan_pub_;
        ros::Publisher goals_pub_;
        std::string frame_id_;

        std::string name_;
        costmap_2d::Costmap2DROS* costmap_ros_;
        costmap_2d::Costmap2D* costmap_;
        double step_size_;
        double min_dist_from_robot_;
        base_local_planner::CostmapModel* world_model_;
        boost::mutex mutex_;
        float convert_offset_ = 0;
        BLH2locpos blh2locpos_;
        std::string path_points_path;


    };

};

#endif